#include "ardoran.h"

//ARDORAN
bool checkBase(PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target, Eigen::Vector4f min, Eigen::Vector4f max, int minPercentage, float maxScore, bool verbose)
{
    time_t start,end;
    if(verbose)
    {
        time(&start);
        cout << "Checking base connection..." << endl;
    }

    // Compute points in the given volume
    Eigen::Vector4f min_pt=min;
    Eigen::Vector4f max_pt=max;
    vector< int >  sourceIndices, targetIndices;
    getPointsInBox<PointXYZRGB>(*source, min_pt, max_pt, sourceIndices);
    getPointsInBox<PointXYZRGB>(*target, min_pt, max_pt, targetIndices);

    // POINT NUMBER CHECK - Check by number of points in the given volume
    bool pointNumberCheckPassed;
    if(sourceIndices.size() > targetIndices.size())
        pointNumberCheckPassed = true;
    else if ( 100*sourceIndices.size()/targetIndices.size() > minPercentage)
        pointNumberCheckPassed = true;
    else pointNumberCheckPassed = false;

    PointCloud<PointXYZRGB>::Ptr sourcePlug (new PointCloud<PointXYZRGB>);
    PointCloud<PointXYZRGB>::Ptr targetPlug (new PointCloud<PointXYZRGB>);
    copyPointCloud(*source, sourceIndices, *sourcePlug);
    copyPointCloud(*target, targetIndices, *targetPlug);

    // INCLINATION CHECK - Check by registering the two plug clouds
    bool inclinationCheckPassed = false;
    float angle = 0;
    if(pointNumberCheckPassed)
    {
        Eigen::Matrix4f transformation = getTransformation(sourcePlug, targetPlug);
        Eigen::Matrix3f rotationMatrix = transformation.topLeftCorner(3,3);
        Eigen::Vector3f eulerAngles = rotationMatrix.eulerAngles(0, 1, 2);
        Eigen::Matrix3f inclinationMatrix;
        inclinationMatrix = Eigen::AngleAxisf(eulerAngles[0], Eigen::Vector3f::UnitX()) *
                Eigen::AngleAxisf(eulerAngles[1],  Eigen::Vector3f::UnitY()) *
                Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitZ()); // just keep the x-y rotation
        Eigen::AngleAxisf angleAxis(inclinationMatrix); // calculate inclination from x-y rotation
        cout << "Inclination Axis : " << endl << angleAxis.axis() << endl;
        cout << "Inclination Angle [rad] : " << angleAxis.angle() << endl; // angolo di inclinazione stimato dalla registrazione
        angle = rad2deg(angleAxis.angle());
        if( angle < 4)
            inclinationCheckPassed = true;
    }

    if(verbose)
    {
        cout << "Source base points number: " << sourceIndices.size() << endl;
        cout << "Target base points number: " << targetIndices.size() << endl;
        cout << "Base points difference: " << abs((int)targetIndices.size() - (int)sourceIndices.size()) << endl;
        cout << "Base points number percentage: " << 100*sourceIndices.size()/targetIndices.size() << endl;
        if(pointNumberCheckPassed) cout << "Base points-number-check: true" << endl;
        else cout << "Base points-number-check: false" << endl;
        if(pointNumberCheckPassed) cout << "Inclination difference is: " << angle << " degrees" << endl;
        if(inclinationCheckPassed) cout << "Returning: true" << endl;
        else cout << "Returning: false" << endl;
        time (&end);
        double diff = difftime (end,start);
        cout << "Checking base took " << diff << " seconds" << endl;
    }

    return inclinationCheckPassed;
}

Eigen::Vector3i getAverageColor(PointCloud<PointXYZRGB>::Ptr cloud, vector<int>* indices)
{
    PointCloud<PointXYZRGB>::Ptr rgbCloud(new PointCloud<PointXYZRGB>);
    copyPointCloud(*cloud,*indices,*rgbCloud);
    // Calculate the average
    Eigen::Vector3i averageRGBColor = rgbCloud->points.at(0).getRGBVector3i();
    for(size_t i = 1; i < rgbCloud->size(); i++)
        averageRGBColor += rgbCloud->points.at(i).getRGBVector3i();
    averageRGBColor = averageRGBColor/rgbCloud->size();
    return averageRGBColor;
}

bool checkSide(PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target, Eigen::Vector4f min, Eigen::Vector4f max, int minPercentage, int maxColorDiff, bool verbose)
{

    time_t start,end;
    if(verbose)
    {
        time(&start);
        cout << "Checking side connection..." << endl;
    }

    // Compute points in the given volume
    Eigen::Vector4f min_pt=min;
    Eigen::Vector4f max_pt=max;
    vector< int >  sourceIndices, targetIndices;
    getPointsInBox<PointXYZRGB>(*source, min_pt, max_pt, sourceIndices);
    getPointsInBox<PointXYZRGB>(*target, min_pt, max_pt, targetIndices);

    // POINT NUMBER CHECK - Check by number of points in the given volume
    bool pointNumberCheckPassed;
    if(sourceIndices.size() > targetIndices.size())
        pointNumberCheckPassed = true;
    else if ( 100*sourceIndices.size()/targetIndices.size() > minPercentage)
        pointNumberCheckPassed = true;
    else pointNumberCheckPassed = false;

    // AVERAGE COLOR CHECK - Check by the average color of the points in the given volume
    bool colorCheckPassed = false;
    Eigen::Vector3i sourceAverageColor, targetAverageColor;
    int colorDiff;
    if(pointNumberCheckPassed)
    {
        sourceAverageColor = getAverageColor(source, &sourceIndices);
        targetAverageColor = getAverageColor(target, &targetIndices);
        colorDiff = abs(targetAverageColor[0] - sourceAverageColor[0]) + abs(targetAverageColor[1] - sourceAverageColor[1]) + abs(targetAverageColor[2] - sourceAverageColor[2]);
        if(colorDiff <= maxColorDiff)
            colorCheckPassed = true;
    }

    if(verbose)
    {
        cout << "Source side points number: " << sourceIndices.size() << endl;
        cout << "Target side points number: " << targetIndices.size() << endl;
        cout << "Side points difference: " << abs((int)targetIndices.size() - (int)sourceIndices.size()) << endl;
        cout << "Side points number percentage: " << 100*sourceIndices.size()/targetIndices.size() << endl;
        if(pointNumberCheckPassed) cout << "Side points-number-check: true" << endl;
        else cout << "Side points-number-check: false" << endl;
        if(pointNumberCheckPassed) cout << "Source average color: r:" << sourceAverageColor[0] << " g:" << sourceAverageColor[1] << " b:" << sourceAverageColor[2] << endl;
        if(pointNumberCheckPassed) cout << "Target average color: r:" << targetAverageColor[0] << " g:" << targetAverageColor[1] << " b:" << targetAverageColor[2] << endl;
        if(pointNumberCheckPassed) cout << "Color Difference: " << colorDiff << endl;
        if(colorCheckPassed) cout << "Returning: true" << endl;
        else cout << "Returning: false" << endl;
        time (&end);
        cout << "Checking side took " << difftime(end,start) << " seconds" << endl;
    }

    return colorCheckPassed;
}

Eigen::Matrix4f getTransformation(PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target)
{
    PointCloud<PointXYZRGB>::Ptr registerdComponentCloud (new PointCloud<PointXYZRGB>);
    // Given that the main clouds are already registered we can just perform the final registration on the clouds of our components to find the transformation.
    IterativeClosestPoint<PointXYZRGB, PointXYZRGB>::Ptr registrator (new IterativeClosestPoint<PointXYZRGB, PointXYZRGB>);
    registrator->setInputCloud(source);
    registrator->setInputTarget(target);
    registrator->setMaxCorrespondenceDistance(4); // nuovo da tunare
    registrator->setRANSACOutlierRejectionThreshold (0.05);
    registrator->setRANSACIterations(1000);
    registrator->setMaximumIterations (100); // 1st stop criterion
    registrator->setTransformationEpsilon (1e-7); // 2nd stop criterion (minumum difference between actual and previous transformation to continue)
    registrator->align(*registerdComponentCloud);
    Eigen::Matrix4f transformation = registrator->getFinalTransformation();
    return transformation;
}
